﻿#include "agv_drv_parser.h"
#include <limits>

#define PROTOCOL_HEADER 0XFEFEFEFE
#define PROTOCOL_END    0XEE

#define PROTOCL_DATA_SIZE 77
#pragma pack(push)
#pragma pack(1)

typedef struct __Mpu6050_Str_ {
  short X_data;
  short Y_data;
  short Z_data;
} Mpu6050_Str;

typedef struct __Moto_Str_ {
  float Moto_CurrentSpeed;
  float Moto_TargetSpeed;
} Moto_Str;

// 00 00 00 80 00 00 00 80 00 00 00 00 00 00 00 80

typedef union _Upload_Data_ {
  unsigned char buffer[PROTOCL_DATA_SIZE];
  struct _Sensor_Str_ {
    unsigned int Header;  // EE FE FE FE FE
    float X_speed;        // 00 00 00 00
    float Y_speed;        // 00 00 00 00
    float Z_speed;        // 00 00 00 00
    float Source_Voltage; // D7 E3 47 41

    Mpu6050_Str Link_Accelerometer; // C4 06 F0 FD 6C 38
    Mpu6050_Str Link_Gyroscope;     // D8 FF 03 00 EB FF

    Moto_Str MotoStr[4]; //
    float PID_Param[3];  // 00 00 00 00 00 00 00 00 00 00 00 00

    unsigned char End_flag; // EE
  } Sensor_Str;
} Upload_Data;

//#pragma pack(4)
#pragma pack(pop)



agv_drv_parser::agv_drv_parser()
{
}

agv_drv_parser::~agv_drv_parser()
{
}

void agv_drv_parser::ProcessingDatas(char *data, int count)
{
//    qDebug() << "count=" << count;
    int idx = 0;
    frame_list_.clear();
    Upload_Data *pData;
    while (idx < count)
    {
        pData = (Upload_Data *)(&data[idx]);
        if (pData->Sensor_Str.Header==PROTOCOL_HEADER)
        {
            auto size = std::min(count-idx,PROTOCL_DATA_SIZE);
            if(size==PROTOCL_DATA_SIZE){
//                qDebug() << "idx=" << idx;
                unpack((unsigned char *)&data[idx], PROTOCL_DATA_SIZE, idx, idx + PROTOCL_DATA_SIZE-1);
                idx += PROTOCL_DATA_SIZE;
            }
        }
        idx++;
    }
}
void agv_drv_parser::unpack(unsigned char *data, int pack_size, int start_idx, int end_idx)
{
    Frame frame;
    frame.image_size_ = 0;
    frame.start_index_ = start_idx;
    frame.end_index_ = end_idx;
    //Upload_Data Recive_Data;
    Upload_Data *pData;

        pData = (Upload_Data *)data;
        frame.is_valid_ = true;
        frame.datas_.append((float)pData->Sensor_Str.X_speed); //0
        frame.datas_.append((float)pData->Sensor_Str.Y_speed); //1
        frame.datas_.append((float)pData->Sensor_Str.Z_speed); //2
        frame.datas_.append((float)pData->Sensor_Str.Source_Voltage); //3
        frame.datas_.append((float)pData->Sensor_Str.Link_Accelerometer.X_data); //4
        frame.datas_.append((float)pData->Sensor_Str.Link_Accelerometer.Y_data); //5
        frame.datas_.append((float)pData->Sensor_Str.Link_Accelerometer.Z_data); //6
        frame.datas_.append((float)pData->Sensor_Str.Link_Gyroscope.X_data); //7
        frame.datas_.append((float)pData->Sensor_Str.Link_Gyroscope.Y_data); //8
        frame.datas_.append((float)pData->Sensor_Str.Link_Gyroscope.Z_data); //9

        frame.datas_.append((float)pData->Sensor_Str.MotoStr[0].Moto_CurrentSpeed); //10
        frame.datas_.append((float)pData->Sensor_Str.MotoStr[0].Moto_TargetSpeed); //11
        frame.datas_.append((float)pData->Sensor_Str.MotoStr[1].Moto_CurrentSpeed); //12
        frame.datas_.append((float)pData->Sensor_Str.MotoStr[1].Moto_TargetSpeed); //13
        frame.datas_.append((float)pData->Sensor_Str.MotoStr[2].Moto_CurrentSpeed); //14
        frame.datas_.append((float)pData->Sensor_Str.MotoStr[2].Moto_TargetSpeed); //15
        frame.datas_.append((float)pData->Sensor_Str.MotoStr[3].Moto_CurrentSpeed); //16
        frame.datas_.append((float)pData->Sensor_Str.MotoStr[3].Moto_TargetSpeed);  //17
        frame_list_.append(frame);


}
